#!/usr/bin/env python
# -*- coding:utf-8 -*-
import PipeColumn as PC
import Blocks as BK
import UI
from Astar import MotionPlaner

ui = UI.UI()
# 获取边界
def getBoundary(pipeArray, blockArray, cellHeight, maxHeight, installationSpace, insulatingSpace):
    if cellHeight == -float("inf"):
        cellHeight = 0
    if maxHeight == float("inf"):
        maxHeight = 0
    minx = pipeArray[0][0][0]
    miny = pipeArray[0][0][1]
    maxx = pipeArray[0][0][0]
    maxy = pipeArray[0][0][1]
    if len(blockArray) != 0:
        minx = blockArray[0][0][0]
        miny = blockArray[0][0][1]
        maxx = blockArray[0][0][0]
        maxy = blockArray[0][0][1]
    minz = cellHeight - 1500
    maxz = maxHeight
    max_width_height = 0
    for pipe in blockArray:
        minx = min(minx, pipe[0][0])
        minx = min(minx, pipe[1][0])
        miny = min(miny, pipe[0][1])
        miny = min(miny, pipe[1][1])
        maxx = max(maxx, pipe[0][0])
        maxx = max(maxx, pipe[1][0])
        maxy = max(maxy, pipe[0][1])
        maxy = max(maxy, pipe[1][1])
        minz = min(minz, pipe[0][2])
        minz = min(minz, pipe[1][2])
        maxz = max(maxz, pipe[1][2])
        maxz = max(maxz, pipe[0][2])
        max_width_height = max(max_width_height, pipe[2])
        max_width_height = max(max_width_height, pipe[3])
    for pipe in pipeArray:
        minx = min(minx, pipe[0][0])
        minx = min(minx, pipe[1][0])
        miny = min(miny, pipe[0][1])
        miny = min(miny, pipe[1][1])
        maxx = max(maxx, pipe[0][0])
        maxx = max(maxx, pipe[1][0])
        maxy = max(maxy, pipe[0][1])
        maxy = max(maxy, pipe[1][1])   
        minz = min(minz, pipe[0][2])
        minz = min(minz, pipe[1][2])
        maxz = max(maxz, pipe[1][2])
        maxz = max(maxz, pipe[0][2])
        max_width_height = max(max_width_height, pipe[2])
        max_width_height = max(max_width_height, pipe[3])
        
    minz -= max_width_height
    max_width_height += installationSpace + insulatingSpace # 取管道和障碍物宽高最大值以及UI参数绝缘层和安装空间之和，用这个值扩大边界，防止栅格化时出现负数坐标
    return [minx - max_width_height, miny - max_width_height, minz,
        maxx + max_width_height, maxy + max_width_height, maxz]

# 输入：管道列、管道列周围障碍物、绝缘层、安装空间、地板高度、假天花高度、天花高度
def astar(pipeColumn, blocks, insulatingSpace, installationSpace, floorHeight, cellHeight, maxHeight):
    # ui = UI.UI() # 全局UI参数
    boundary = getBoundary(pipeColumn, blocks, cellHeight, maxHeight, installationSpace, insulatingSpace) # 获取边界
    p = PC.PipeColumn(pipeColumn, boundary, insulatingSpace) # 构造管道列对象
    #print(p.getGrid())

    w = BK.WallBlocks(blocks, boundary, p.get_width_heigh()[0], p.get_width_heigh()[1], installationSpace) # 构造障碍物对象
    #print(w.getGrid())

    # A*输入
    pipe_girds = p.getGrid() # 管道列栅格
    block_grids = w.getWallGrid() # 障碍物栅格
    single_grids = w.getSingleGrid() # 单点障碍物栅格
    boundarys = p.getGridBoundary() # 栅格化之后的边界[maxx, maxy, maxz]
    astar_grid = [[]] # A*寻径后的栅格路径
    #print(p.shortPipe)

    # A*算法
    # grid: [起点、终点、中间点]
    for grid in pipe_girds:
        #print(grid)
        motion_planer = MotionPlaner([block_grids], single_grids, grid[0], grid[1], boundarys)  # 输入障碍物数据，起点，终点
        if len(grid) == 3:
            voxmap, out = motion_planer.plot3DModel([block_grids], [], grid[0], grid[1], [grid[2]], 0)
            astar_grid.extend(out)
        else:
            voxmap, out = motion_planer.plot3DModel([block_grids], [], grid[0], grid[1], [], 0)
            astar_grid.extend(out)

        
    #print(astar_grid)
    p.Grid_to_Point(astar_grid[1:]) # 栅格转坐标
    # 管道列输出
    routes = p.getRoute()
    #print(routes)
    return routes


# 输入：管道列、管道列周围障碍物、绝缘层、安装空间、地板高度、假天花高度、天花高度
def get_short_pipe(pipeColumn, blocks, insulatingSpace, installationSpace, floorHeight, cellHeight, maxHeight):
    boundary = getBoundary(pipeColumn, blocks, cellHeight, maxHeight, installationSpace, insulatingSpace) # 获取边界
    p = PC.PipeColumn(pipeColumn, boundary, insulatingSpace) # 构造管道列对象
    return p.shortPipe




if __name__ == "__main__":


    pipes = [
    [[38371.863314706, -11707.113251366, 1525.0], [38487.877818712, -12000.383708644, 1525.0], 1600.0, 3000.0, None], 
    [[39469.085826654, -12781.274124864, 500.0], [40163.928511296, -12506.25123241, 500.0], 950.0, 950.0, None], 
    [[40745.063083347, -12276.234691338, -125.0], [40745.063083347, -12276.234691338, -385.0], 950.0, 950.0, 0.37688562578817697]
    ]            
    

    # 管道列周围建筑物
    walls = [
    [[39132.2724261835, 4578.742795903, -5800.0], [39132.2724261835, 4578.742795903, -650.0], 1315.714977735, 1709.11327535], 
    [[41337.6952268735, -16218.338991996001, -5800.0], [41337.6952268735, -16218.338991996001, 600.0], 1019.377003331, 1356.575543456], 
    [[47195.9606503905, -13900.72763874, -5800.0], [47195.9606503905, -13900.72763874, -175.0], 1019.377003331, 1356.575543456], 
    [[46355.2897191035, -11775.904447539, -5800.0], [46355.2897191035, -11775.904447539, -175.0], 1019.377003331, 1356.575543456], 
    [[45440.305089714, -18016.6487476885, -5800.0], [45440.305089714, -18016.6487476885, 1500.0], 1356.577838652, 1019.394644363]
    ]

    boundary = getBoundary(pipes, walls, ui.cellHeight, ui.maxHeight, ui.installationSpace, ui.insulatingSpace) # 获取边界

    p = PC.PipeColumn(pipes, boundary, ui.insulatingSpace) # 构造管道列对象
    print(p.getGrid())

    w = BK.WallBlocks(walls, boundary, p.get_width_heigh()[0], p.get_width_heigh()[1], ui.installationSpace) # 构造障碍物对象
    print(w.getWallGrid())


    # A*输入
    pipe_girds = p.getGrid() # 管道列栅格
    block_grids = w.getWallGrid() # 障碍物栅格
    single_grids = w.getSingleGrid() # 单点障碍物栅格
    boundarys = p.getGridBoundary() # 栅格化之后的边界[maxx, maxy, maxz]
    astar_grid = [[]] # A*寻径后的栅格路径
    print(p.shortPipe)

    # A*算法
    # grid: [起点、终点、中间点]
    for grid in pipe_girds:
        #print(grid)
        motion_planer = MotionPlaner([block_grids], single_grids, grid[0], grid[1], boundarys)  # 输入障碍物数据，起点，终点
        if len(grid) == 3:
            voxmap, out = motion_planer.plot3DModel([block_grids], [], grid[0], grid[1], [grid[2]], 0)
            astar_grid.extend(out)
        else:
            voxmap, out = motion_planer.plot3DModel([block_grids], [], grid[0], grid[1], [], 0)
            astar_grid.extend(out)

        
    print(astar_grid)
    p.Grid_to_Point(astar_grid[1:]) # 栅格转坐标
    # 管道列输出
    routes = p.getRoute()
    print(routes)
